#include <behaviortree_cpp_v3/bt_factory.h>
#include <iostream>
#include <chrono>
#include <thread>
using namespace std;

class ApproachObject : public BT::SyncActionNode
{
public:
    ApproachObject(const string& name)
        : BT::SyncActionNode(name, {})
    {
    }

    BT::NodeStatus tick() override
    {
        cout << this->name() << ": Apporach" << endl;
        this_thread::sleep_for(chrono::seconds(1));
        return BT::NodeStatus::SUCCESS;
    }
};

class CheckBattery : public BT::SyncActionNode
{
public:
    CheckBattery(const string& name)
        : BT::SyncActionNode(name, {})
    {
    }

    BT::NodeStatus tick() override
    {
        cout << this->name() << ": CheckBattery" << endl;
        this_thread::sleep_for(chrono::seconds(1));
        return BT::NodeStatus::SUCCESS;
    }
};

class GripperInterface
{
private:
    bool open_ = false;

public:
    GripperInterface()
        : open_(false)
    {
    }

    BT::NodeStatus open()
    {
        cout << "gripper open" << endl;
        open_ = true;
        return BT::NodeStatus::SUCCESS;
    }
    BT::NodeStatus close()
    {
        cout << "gripper close" << endl;
        open_ = false;
        return BT::NodeStatus::SUCCESS;
    }
};


int main()
{
    // 行为工厂
    BT::BehaviorTreeFactory factory;
    // 注册节点类型
    factory.registerNodeType<ApproachObject>("ApproachObject");
    factory.registerNodeType<CheckBattery>("CheckBattery");
    // 注册带参数的节点
    GripperInterface gripper;
    factory.registerSimpleAction("OpenGripper", [&gripper](BT::TreeNode&) {
        return gripper.open();
        });
    factory.registerSimpleAction("CloseGripper", [&gripper](BT::TreeNode&) {
        return gripper.close();
        });
    // 定义行为树 xml结构
    string xml_tree = R"(
<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
            <CheckBattery name="check_battery" />
            <ApproachObject name="approach_object" />
            <OpenGripper name="open_gripper" />
            <ApproachObject name="approach_object_again" />
            <CloseGripper name="close_gripper" />
        </Sequence>
    </BehaviorTree>
</root>
    )";
    // 从xml 加载行为树
    BT::Tree tree = factory.createTreeFromText(xml_tree);

    // 打印行为树结构到标准输出
    cout << "=== 行为树结构 ===" << endl;
    printTreeRecursively(tree.rootNode());

    // 执行行为树
    tree.tickRoot();
    return 0;
}

/* 
=== 行为树结构 ===
----------------
root_sequence
   check_battery
   approach_object
   open_gripper
   approach_object_again
   close_gripper
----------------
check_battery: CheckBattery
approach_object: Apporach
gripper open
approach_object_again: Apporach
gripper close
*/